#include "ros/ros.h"
#include <tf/tf.h>
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"

//////////////////////////////////////////////////////////////////////////////

double Pi = 3.14159265;
double yaw, pitch, roll;
double Std_odom_x = 0, Std_odom_w = 0, Std_vel_x = 0, Std_vel_w = 0;  // std_:standard value;
double Acl_odom_x = 0, Acl_odom_w = 0, Acl_vel_x = 0, Acl_vel_w = 0;  // acl_:actual value.
double Acl_odom_y = 0, Acl_odom_Vx = 0, Acl_odom_Vw = 0;
geometry_msgs::Twist init_vel, set_vel;  

//回调函数：返回里程计信息
void write_odom_callback (const nav_msgs::Odometry msg)
{
    // 四元数转欧拉角
    tf::Matrix3x3 mat(tf::Quaternion((msg.pose.pose.orientation.x), 
                                     (msg.pose.pose.orientation.y), 
                                     (msg.pose.pose.orientation.z), 
                                     (msg.pose.pose.orientation.w)));    
    
    mat.getEulerYPR(yaw, pitch, roll);

    Acl_odom_y = msg.pose.pose.position.y;
    Acl_odom_Vx = msg.twist.twist.linear.x;
    Acl_odom_Vw = msg.twist.twist.angular.z;

    if (set_vel.linear.x != 0)
    {
        Acl_odom_x = msg.pose.pose.position.x;
    }
    if (set_vel.angular.z != 0)
    {
        Acl_odom_w = yaw / Pi * 180;  //弧度值 转 角度值
    }

    static double last_odom_w = 0;
    if (Std_vel_w > 0)
    {
        if (Acl_odom_w < 0)
        {
            Acl_odom_w = last_odom_w + Acl_odom_w + Pi;
        }
        else 
        {
            Acl_odom_w = last_odom_w + Acl_odom_w;
        }
    
        last_odom_w = Acl_odom_w;
    }
    else if (Std_vel_w < 0)
    {

        if (Acl_odom_w > 0)
        {
            Acl_odom_w = last_odom_w + Acl_odom_w - Pi;
        }
        else 
        {
            Acl_odom_w = last_odom_w + Acl_odom_w;
        }
    
        last_odom_w = Acl_odom_w;
    }
}

///////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "caliodom");
    //声明节点句柄
    ros::NodeHandle nh_1;

    //订阅主题，并配置回调函数
    ros::Subscriber sub_odom = nh_1.subscribe("/odom", 1000, write_odom_callback);
    //发布主题
    ros::Publisher pub_vel = nh_1.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);

    //判断是否传入参数
    if(ros::param::has("~std_Vx"))
    {
        //传入参数赋值
       	ros::param::get("~std_Vx", Std_vel_x );
        ros::param::get("~std_Lx", Std_odom_x);

        set_vel.linear.x = Std_vel_x;
   	}
    else
    {
        set_vel.linear.x = 0.0;
    }   
       
    if(ros::param::has("~std_Vw"))
    {
       	ros::param::get("~std_Vw", Std_vel_w );
        ros::param::get("~std_Lw", Std_odom_w);

        set_vel.angular.z = Std_vel_w;
   	}
    else
    {
        set_vel.angular.z = 0.0;
    }
    // ROS_INFO_STREAM("~Std_vel : " << Std_vel);
    // ROS_INFO_STREAM("~Std_odom: " << Std_odom);

    ros::Rate loop_rate(100);                //循环周期：1 / x = s; 1 / 20 = 0.05s = 50ms
    while (ros::ok())
    {
        int Std_odom_x_over = 0;
        int Std_odom_w_over = 0;

        ros::spinOnce();

        ROS_INFO_STREAM("Acl_odom_x: " << Acl_odom_x << "  Acl_odom_y: " << Acl_odom_y << "  Acl_odom_w: " << Acl_odom_w);

        //  Vx 方向运动处理
        if ((set_vel.linear.x > 0) && (Std_odom_x >= 0))
        {
            if (Acl_odom_x >= Std_odom_x)
            {
                ROS_INFO_STREAM("Std_odom_x --- Have done !!!");

                Std_odom_x_over = 1;
                set_vel.linear.x = 0;

                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
            }
        }
        else if ((set_vel.linear.x < 0) && (Std_odom_x <= 0))
        {
            if (Acl_odom_x <= Std_odom_x)
            {
                ROS_INFO_STREAM("Std_odom_x --- Have done !!!");

                Std_odom_x_over =1;
                set_vel.linear.x = 0;

                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
            }
        }
        else
        {
            Std_odom_x_over = 1;
            set_vel.linear.x = 0;
            pub_vel.publish(set_vel);
        }
        //  Vw 方向运动处理
        if ((set_vel.angular.z > 0) && (Std_odom_w >= 0))
        {
            if (Acl_odom_w >= Std_odom_w)
            {
                ROS_INFO_STREAM("Std_odom_w --- Have done !!!");

                Std_odom_w_over = 1;
                set_vel.angular.z = 0;

                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
            }
        }
        else if ((set_vel.angular.z < 0) && (Std_odom_w <= 0))
        {
            if (Acl_odom_w <= Std_odom_w)
            {
                ROS_INFO_STREAM("Std_odom_w --- Have done !!!");

                Std_odom_w_over = 1;
                set_vel.angular.z = 0;

                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
                pub_vel.publish(set_vel);
            }
        }
        else
        {
            Std_odom_w_over = 1;
            set_vel.angular.z = 0;
            pub_vel.publish(set_vel);
        }

        if (Std_odom_w_over && Std_odom_x_over)
        {
            while ((Acl_odom_Vx != 0) && (Acl_odom_Vw != 0))
            {
                ros::spinOnce();
                // ROS_INFO_STREAM("Acl_odom_Vx:" << Acl_odom_Vx << "  Acl_odom_Vw:" << Acl_odom_Vw);
                pub_vel.publish(init_vel);
            }

            ROS_INFO_STREAM("ALL Have done --- break !!!");
            break;
        }
        
        loop_rate.sleep();
    }


}